{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "51d7efc2",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[1. 0.]\n",
      " [0. 1.]]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "a = np.matrix([[1,2],[-1,-3]])\n",
    "b = np.linalg.inv(a)\n",
    "print(a@b)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0596e85e",
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_inv_matrix(frame):\n",
    "    with open(os.path.join(calib_path, frame+\".txt\")) as f:\n",
    "        lines = f.readlines()\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"Tr_velo_to_cam\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]\n",
    "        matrix = matrix + [0,0,0,1]\n",
    "        m = np.array(matrix)\n",
    "        velo_to_cam  = m.reshape([4,4])\n",
    "\n",
    "\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"R0_rect\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]        \n",
    "        m = np.array(matrix).reshape(3,3)\n",
    "        \n",
    "        m = np.concatenate((m, np.expand_dims(np.zeros(3), 1)), axis=1)\n",
    "        \n",
    "        rect = np.concatenate((m, np.expand_dims(np.array([0,0,0,1]), 0)), axis=0)        \n",
    "        \n",
    "        \n",
    "        m = np.matmul(rect, velo_to_cam)\n",
    "\n",
    "        \n",
    "        m = np.linalg.inv(m)\n",
    "        \n",
    "        return m\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c53ecce",
   "metadata": {},
   "outputs": [],
   "source": [
    "# def camera_to_lidar(points, r_rect, velo2cam):\n",
    "#     points_shape = list(points.shape[0:-1])\n",
    "#     if points.shape[-1] == 3:\n",
    "#         points = np.concatenate([points, np.ones(points_shape + [1])], axis=-1)\n",
    "#     lidar_points = points @ np.linalg.inv((r_rect @ velo2cam).T)\n",
    "#     return lidar_points[..., :3]\n",
    "\n",
    "\n",
    "# def lidar_to_camera(points, r_rect, velo2cam):\n",
    "#     points_shape = list(points.shape[:-1])\n",
    "#     if points.shape[-1] == 3:\n",
    "#         points = np.concatenate([points, np.ones(points_shape + [1])], axis=-1)\n",
    "#     camera_points = points @ (r_rect @ velo2cam).T\n",
    "#     return camera_points[..., :3]\n",
    "\n",
    "\n",
    "# def box_camera_to_lidar(data, r_rect, velo2cam):\n",
    "#     xyz = data[:, 0:3]\n",
    "#     l, h, w = data[:, 3:4], data[:, 4:5], data[:, 5:6]\n",
    "#     r = data[:, 6:7]\n",
    "#     xyz_lidar = camera_to_lidar(xyz, r_rect, velo2cam)\n",
    "#     return np.concatenate([xyz_lidar, w, l, h, r], axis=1)\n",
    "\n",
    "\n",
    "# def box_lidar_to_camera(data, r_rect, velo2cam):\n",
    "#     xyz_lidar = data[:, 0:3]\n",
    "#     w, l, h = data[:, 3:4], data[:, 4:5], data[:, 5:6]\n",
    "#     r = data[:, 6:7]\n",
    "#     xyz = lidar_to_camera(xyz_lidar, r_rect, velo2cam)\n",
    "#     return np.concatenate([xyz, l, h, w, r], axis=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "d045839a",
   "metadata": {},
   "outputs": [],
   "source": [
    "#生成txt文件\n",
    "import math\n",
    "import json\n",
    "def vel_to_cam(path, name ='000000.txt'):\n",
    "    with open(path + name,'r') as f_txt:\n",
    "        lines = f_txt.readlines()\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"Tr_velo_to_cam\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]\n",
    "        matrix = matrix + [0,0,0,1]\n",
    "        m = np.array(matrix)\n",
    "        velo_to_cam  = m.reshape([4,4])\n",
    "\n",
    "\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"R0_rect\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]        \n",
    "        m = np.array(matrix).reshape(3,3)\n",
    "\n",
    "        m = np.concatenate((m, np.expand_dims(np.zeros(3), 1)), axis=1)\n",
    "\n",
    "        rect = np.concatenate((m, np.expand_dims(np.array([0,0,0,1]), 0)), axis=0)        \n",
    "\n",
    "\n",
    "        m = np.matmul(rect, velo_to_cam)\n",
    "\n",
    "\n",
    "    #     m = np.linalg.inv(m)\n",
    "\n",
    "        return m\n",
    "# m = vel_to_cam(1)\n",
    "# print(m)\n",
    "def json_to_txt(name, out_path):\n",
    "    with open('/home/dk/SUSTechPOINTS/data/my_dataset_all/label/'+name+'.json','r') as f:\n",
    "        obj= json.load(f)\n",
    "    m = vel_to_cam('/home/dk/SUSTechPOINTS/data/my_dataset_all/')\n",
    "    # print(trans)\n",
    "    for i in obj:\n",
    "    #     print(np.array(i['psr']['position']))\n",
    "        pos = np.array([i['psr']['position']['x'], i['psr']['position']['y'],i['psr']['position']['z'] - \n",
    "                        i['psr']['scale']['z']/2, 1]).T\n",
    "    #     pos1 = np.array([i['psr']['position']['x'], i['psr']['position']['y'], i['psr']['position']['z'], 1]).T\n",
    "    #     print(pos, pos1)\n",
    "        trans_pos = np.matmul(m,pos)\n",
    "        height = i['psr']['scale']['z']\n",
    "        length = i['psr']['scale']['y']\n",
    "        width = i['psr']['scale']['x']\n",
    "        rotation_y = -math.pi/2 - i['psr']['rotation']['z']\n",
    "        #print(trans_pos)\n",
    "        #print('Car 0 0 0 455.7 183.86 533.81 241.91 ' + str(round(height,2)) +' '+ str(round(length,2)) +' ' + str(round(width,2)) +' '+ str(round(trans_pos[0],2)) +' '+ str(round(trans_pos[1],2)) +' '+ str(round(trans_pos[2],2)) +' '+ str(round(rotation_y,2)))\n",
    "        if not os.path.exists(out_path):\n",
    "            os.makedirs(out_path)\n",
    "        with open(out_path+name+'.txt','a+') as f2:\n",
    "            f2.writelines('Car 0 0 0 455.7 183.86 533.81 241.91 ' + str(round(height,2)) +' '+ str(round(length,2)) +' ' + str(round(width,2)) +' '+ str(round(trans_pos[0],2)) +' '+ str(round(trans_pos[1],2)) +' '+ str(round(trans_pos[2],2)) +' '+ str(round(rotation_y,2))+'\\n')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "4cd69fb4",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[{'psr': {'position': {'x': 7.906420416239251, 'y': 14.056841648745534, 'z': -0.6361759845167398}, 'scale': {'x': 2.9736764285955974, 'y': 1.606004806541132, 'z': 1.3554377667605877}, 'rotation': {'x': 0, 'y': 0, 'z': -1.597941115041151}}, 'obj_type': 'Car', 'obj_id': ''}]\n",
      "{'obj_type': 'Car', 'psr': {'scale': {'z': 1.36, 'x': 2.97, 'y': 1.61}, 'position': {'x': 7.907210348574928, 'y': 14.05302234830936, 'z': -0.6309184417423447}, 'rotation': {'x': 0, 'y': 0, 'z': -1.6007963267948966}}}\n"
     ]
    }
   ],
   "source": [
    "##查看生成的tst是否正确，对比json\n",
    "import math\n",
    "import json\n",
    "def vel_to_cam(path):\n",
    "    with open('/home/dk/SUSTechPOINTS/data/my_dataset_all/000000.txt','r') as f_txt:\n",
    "        lines = f_txt.readlines()\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"Tr_velo_to_cam\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]\n",
    "        matrix = matrix + [0,0,0,1]\n",
    "        m = np.array(matrix)\n",
    "        velo_to_cam  = m.reshape([4,4])\n",
    "\n",
    "\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"R0_rect\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]        \n",
    "        m = np.array(matrix).reshape(3,3)\n",
    "\n",
    "        m = np.concatenate((m, np.expand_dims(np.zeros(3), 1)), axis=1)\n",
    "\n",
    "        rect = np.concatenate((m, np.expand_dims(np.array([0,0,0,1]), 0)), axis=0)        \n",
    "\n",
    "\n",
    "        m = np.matmul(rect, velo_to_cam)\n",
    "\n",
    "\n",
    "    #     m = np.linalg.inv(m)\n",
    "\n",
    "        return m\n",
    "\n",
    "def cam_to_vel(path):\n",
    "    with open('/home/dk/SUSTechPOINTS/data/my_dataset_all/000000.txt','r') as f_txt:\n",
    "        lines = f_txt.readlines()\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"Tr_velo_to_cam\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]\n",
    "        matrix = matrix + [0,0,0,1]\n",
    "        m = np.array(matrix)\n",
    "        velo_to_cam  = m.reshape([4,4])\n",
    "\n",
    "\n",
    "        trans = [x for x in filter(lambda s: s.startswith(\"R0_rect\"), lines)][0]\n",
    "        matrix = [m for m in map(lambda x: float(x), trans.split(\" \")[1:])]        \n",
    "        m = np.array(matrix).reshape(3,3)\n",
    "\n",
    "        m = np.concatenate((m, np.expand_dims(np.zeros(3), 1)), axis=1)\n",
    "\n",
    "        rect = np.concatenate((m, np.expand_dims(np.array([0,0,0,1]), 0)), axis=0)        \n",
    "\n",
    "\n",
    "        m = np.matmul(rect, velo_to_cam)\n",
    "\n",
    "\n",
    "        m = np.linalg.inv(m)\n",
    "\n",
    "        return m\n",
    "inv_m = cam_to_vel(1)\n",
    "with open('/home/dk/SUSTechPOINTS/data/my_dataset_all/label/000006.json','r') as f:\n",
    "    f1 = f\n",
    "    print(json.load(f1))\n",
    "    with open('/home/dk/SUSTechPOINTS/data/my_dataset_all/label_txt/000006.txt','r') as f2:\n",
    "        l = f2.readlines()[0]\n",
    "        words = l.strip().split(\" \")\n",
    "#         print(words)\n",
    "        obj = {}\n",
    "        pos = np.array([float(words[11]), float(words[12]), float(words[13]), 1]).T\n",
    "        trans_pos = np.matmul(inv_m, pos)\n",
    "        #print(trans_pos)\n",
    "\n",
    "        obj[\"obj_type\"] = words[0]\n",
    "        obj[\"psr\"] = {\"scale\": \n",
    "                       {\"z\":float(words[8]),    #height\n",
    "                         \"x\":float(words[10]),  #length\n",
    "                         \"y\":float(words[9])},  #width\n",
    "                        \"position\": {\"x\":trans_pos[0], \"y\":trans_pos[1], \"z\":trans_pos[2]+float(words[8])/2},\n",
    "                        \"rotation\": {\"x\":0, \n",
    "                                     \"y\":0,\n",
    "                                     \"z\": -math.pi/2 -float(words[14])}}\n",
    "        print(obj)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d891ee4",
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "path = '/home/dk/SUSTechPOINTS/data/my_dataset_all/label'\n",
    "out_path = '/home/dk/SUSTechPOINTS/data/my_dataset_all/label_txt/'\n",
    "list_name = os.listdir(path)\n",
    "for name in list_name:\n",
    "    f_name = name.split('.json')[0]\n",
    "    json_to_txt(f_name, out_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "92a0c1c7",
   "metadata": {},
   "outputs": [],
   "source": [
    "def write_txt(out_path = '/home/dk/SUSTechPOINTS/data/my_dataset_all/calib_kitti'):\n",
    "    with open('/home/dk/SUSTechPOINTS/data/my_dataset_all/000000.txt','r') as f1:\n",
    "    #     f1_ = f1.read()\n",
    "    #     print(f1_)\n",
    "        f1_ = f1.read()\n",
    "        if not os.path.exists(out_path):\n",
    "            os.makedirs(out_path)\n",
    "        for name in range(0,210):\n",
    "            with open(out_path + '/%06d.txt'%name,'w') as f2:\n",
    "#                 print(f1_)\n",
    "                f2.write(f1_)        \n",
    "def have_write_txt(out_path = '/home/dk/SUSTechPOINTS/data/my_dataset_all/have_calib_kitti/'):\n",
    "    all_name = os.listdir('/home/dk/SUSTechPOINTS/data/my_dataset_all/label')\n",
    "    with open('/home/dk/SUSTechPOINTS/data/my_dataset_all/000000.txt','r') as f1:\n",
    "    #     f1_ = f1.read()\n",
    "    #     print(f1_)\n",
    "        f1_ = f1.read()\n",
    "        if not os.path.exists(out_path):\n",
    "            os.makedirs(out_path)\n",
    "        for name in all_name:\n",
    "            name_ = name.split('.json')[0]\n",
    "#             print(name_)\n",
    "            with open(out_path + name_ +'.txt','w') as f2:\n",
    "#                 print(f1_)\n",
    "                f2.write(f1_)        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "4b054c3e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "000383\n",
      "000306\n",
      "000235\n",
      "000367\n",
      "000075\n",
      "000478\n",
      "000256\n",
      "000440\n",
      "000282\n",
      "000205\n",
      "000043\n",
      "000254\n",
      "000279\n",
      "000292\n",
      "000098\n",
      "000023\n",
      "000429\n",
      "000305\n",
      "000370\n",
      "000400\n",
      "000046\n",
      "000007\n",
      "000375\n",
      "000355\n",
      "000283\n",
      "000399\n",
      "000161\n",
      "000076\n",
      "000253\n",
      "000177\n",
      "000445\n",
      "000167\n",
      "000442\n",
      "000395\n",
      "000477\n",
      "000531\n",
      "000245\n",
      "000293\n",
      "000258\n",
      "000051\n",
      "000040\n",
      "000252\n",
      "000308\n",
      "000021\n",
      "000016\n",
      "000026\n",
      "000284\n",
      "000248\n",
      "000354\n",
      "000349\n",
      "000377\n",
      "000391\n",
      "000096\n",
      "000307\n",
      "000037\n",
      "000249\n",
      "000401\n",
      "000210\n",
      "000381\n",
      "000398\n",
      "000444\n",
      "000481\n",
      "000025\n",
      "000031\n",
      "000529\n",
      "000251\n",
      "000353\n",
      "000169\n",
      "000532\n",
      "000162\n",
      "000240\n",
      "000448\n",
      "000164\n",
      "000530\n",
      "000259\n",
      "000039\n",
      "000276\n",
      "000220\n",
      "000172\n",
      "000450\n",
      "000203\n",
      "000250\n",
      "000100\n",
      "000336\n",
      "000397\n",
      "000238\n",
      "000417\n",
      "000244\n",
      "000020\n",
      "000163\n",
      "000335\n",
      "000291\n",
      "000378\n",
      "000018\n",
      "000176\n",
      "000171\n",
      "000382\n",
      "000030\n",
      "000054\n",
      "000394\n",
      "000175\n",
      "000352\n",
      "000294\n",
      "000212\n",
      "000241\n",
      "000295\n",
      "000202\n",
      "000255\n",
      "000435\n",
      "000038\n",
      "000170\n",
      "000173\n",
      "000290\n",
      "000359\n",
      "000357\n",
      "000384\n",
      "000042\n",
      "000428\n",
      "000011\n",
      "000424\n",
      "000050\n",
      "000246\n",
      "000236\n",
      "000044\n",
      "000437\n",
      "000287\n",
      "000426\n",
      "000097\n",
      "000166\n",
      "000019\n",
      "000379\n",
      "000078\n",
      "000079\n",
      "000449\n",
      "000286\n",
      "000008\n",
      "000374\n",
      "000022\n",
      "000237\n",
      "000281\n",
      "000099\n",
      "000332\n",
      "000480\n",
      "000165\n",
      "000419\n",
      "000416\n",
      "000309\n",
      "000053\n",
      "000077\n",
      "000479\n",
      "000247\n",
      "000333\n",
      "000368\n",
      "000285\n",
      "000425\n",
      "000174\n",
      "000029\n",
      "000243\n",
      "000024\n",
      "000204\n",
      "000257\n",
      "000373\n",
      "000365\n",
      "000441\n",
      "000392\n",
      "000168\n",
      "000233\n",
      "000052\n",
      "000006\n",
      "000436\n",
      "000515\n",
      "000234\n",
      "000211\n",
      "000009\n",
      "000041\n",
      "000045\n",
      "000027\n",
      "000350\n",
      "000366\n",
      "000331\n",
      "000421\n",
      "000239\n",
      "000427\n",
      "000385\n",
      "000351\n",
      "000439\n",
      "000028\n",
      "000080\n",
      "000232\n",
      "000206\n",
      "000334\n",
      "000010\n",
      "000372\n",
      "000371\n",
      "000209\n",
      "000420\n",
      "000017\n",
      "000208\n",
      "000376\n",
      "000364\n",
      "000049\n",
      "000369\n",
      "000032\n",
      "000356\n",
      "000443\n",
      "000438\n",
      "000451\n",
      "000396\n",
      "000242\n",
      "000358\n"
     ]
    }
   ],
   "source": [
    "# write_txt()\n",
    "have_write_txt()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "30ffe6cf",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "eec99856",
   "metadata": {},
   "outputs": [],
   "source": [
    "#分训练测试集\n",
    "import os\n",
    "path = '/home/dk/SUSTechPOINTS/data/my_dataset_all/label_txt'\n",
    "f_train = open('/home/dk/SUSTechPOINTS/data/my_dataset_all/train.txt','w')\n",
    "f_val = open('/home/dk/SUSTechPOINTS/data/my_dataset_all/val.txt','w')\n",
    "f_trainval = open('/home/dk/SUSTechPOINTS/data/my_dataset_all/train_val.txt','w')\n",
    "f_test = open('/home/dk/SUSTechPOINTS/data/my_dataset_all/test.txt','w')\n",
    "enumerate\n",
    "list_name = os.listdir(path)\n",
    "for i,name in enumerate(list_name):\n",
    "    f_name = name.split('.txt')[0]\n",
    "    f_trainval.write(str(f_name)+'\\n')\n",
    "    f_test.write(str(f_name)+'\\n')\n",
    "    if i % 8 ==0:\n",
    "        f_val.write(str(f_name)+'\\n')\n",
    "    else:\n",
    "        f_train.write(str(f_name)+'\\n')\n",
    "#     print(i,f_name)\n",
    "f_train.close()\n",
    "f_test.close()\n",
    "f_trainval.close()\n",
    "f_val.close()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
